Consistent unscented incremental smoothing for multi-robot cooperative target tracking

In this paper, we study the problem of multi-robot cooperative target tracking, where a team of mobile robots cooperatively localize themselves and track (multiple) targets using their onboard sensor measurements as well as target stochastic kinematic information, and which is hence termed cooperative localization and target tracking (CLATT). A novel efficient, consistent, unscented incremental smoothing (UIS) algorithm is introduced. The key idea of the proposed approach is that we employ unscented transform to numerically compute Jacobians so as to attain reduced linearization errors, while further imposing appropriate constraints on unscented transform to ensure correct observability properties for the incrementally-linearized system. In particular, for the first time we analyze the observability properties of the optimal batch maximum a posteriori (MAP)-based CLATT system, and show that the Fisher information (Hessian) matrix without prior has a nullspace of dimension three, corresponding to the global state information. However, this may not be the case when the Jacobians (and thus the Hessian) are computed canonically by the standard unscented transform, thus negatively impacting the estimation performance. To address this issue, we formulate an observability-constrained unscented transform, and find its closed-from solution as the projection of the canonical unscented Jacobian (i.e., computed by the standard unscented transform) onto an appropriate observable subspace such that the resulting Hessian has a nullspace of correct dimenEmail addresses: [email protected] (Guoquan Huang), [email protected] (Michael Kaess), [email protected] (John J. Leonard) Preprint submitted to Robotics and Autonomous Systems August 25, 2014 sions. The proposed approach is tested extensively through Monte Carlo simulations as well as a real-world experiment, and is shown to outperform the state-of-the-art incremental smoothing algorithm.